//
// Created by hrg on 2019/11/23.
//
#include <slave.h>
#include <cassert>
#include <ecrt/master.h>
#include <slave_config.h>

uint16_t Slave::alias_ = 0;
uint16_t Slave::realpos_ = 0;

Slave::Slave()
    : m_SlvConfig_(NULL)
    , m_Master_(NULL)
    , m_PdoEntry_(NULL)
    , m_EEprom_(NULL)
{
    m_PdoEntryOffset_ = NULL;

    m_Used_ = true;
    enable_= false;
    reset_= false;

    m_Index_ = 0;

    delay_ = 0;
    alias_ = 0;
    realpos_ = 0;
    m_PdoNum_ = 0;

    m_SlaveType_ = EC_OTHER;    // default
    m_SyncInfo_ = NULL;
}

Slave::Slave(ec_master_t*, int)
{}

Slave::~Slave()
{
    if(m_SyncInfo_)
    {
        delete[] m_SyncInfo_;
        m_SyncInfo_ = NULL;

        delete m_EEprom_;
        m_EEprom_ = NULL;
    }

    if(m_PdoEntry_)
    {
        delete[] m_PdoEntry_;
        m_PdoEntry_ = NULL;

        delete[] m_PdoEntryOffset_;
        m_PdoEntryOffset_ = NULL;

        delete[] m_PdoBitPos_;
        m_PdoBitPos_ = NULL;

        m_PdoOffset_.clear();
    }
}

bool Slave::Parse(EcXml &ecxml, ec_master_t* master, int index)
{
    return true;
}

EcType Slave::Type(EcXml& ecxml, ec_master_t* master, int index)
{
    ec_slave_info_t slvinfo;
    ecrt_master_get_slave(master, index, &slvinfo);

    const char* ProfileNode = ecxml.ProfileNo();
    if ( ProfileNode && !strcmp(ProfileNode, "402") )
    {
        return EC_SERVO;
    }

    switch(slvinfo.vendor_id)
    {
        case ECIO_WELL:
            return EC_WELL_DIGITAL_IO;
        case ECIO_INEXBOT:
            return EC_INEXBOT_IO;
        case ECIO_BECKHOFF:
            return EC_BECKHOFF_IO;
        case ECIO_NOWIO:
        	return EC_NOWIO;
        default:
            return EC_IO;
    }

    return EC_OTHER;
}

bool Slave::Match(EcXml& ecxml, ec_master_t* master, int index)
{
    ec_slave_info_t slvinfo;
    ecrt_master_get_slave(master, index, &slvinfo);
    if(ecxml.match(slvinfo))
    {
        return true;
    }
    return false;
}

bool Slave::Init(ec_master_t* master, int index)
{
    if(m_Master_ != NULL)
    {
        return false;
    }

    m_Master_ = master;
    m_Index_ = index;
    m_EEprom_ = new EEprom;

    rxpdo_.mode = 0x08;
    rxpdo_.control = 0;
    rxpdo_.pos = 0;

    ecrt_master_get_slave(m_Master_, m_Index_, &m_SlvInfo_);
#if 0
    if (m_SlvInfo_.alias == 255)
    {
        alias_ = m_SlvInfo_.alias;
        realpos_ = 0;
        m_SlvInfo_.position = realpos_;
    }
    else
    {
        m_SlvInfo_.alias = alias_;
        m_SlvInfo_.position = realpos_ ++;
    }
#endif
    m_SlvConfig_ = ecrt_master_slave_config(m_Master_, 0, m_SlvInfo_.position, m_SlvInfo_.vendor_id, m_SlvInfo_.product_code);
    if (m_SlvConfig_ == nullptr)
    {
        return false;
    }
    return true;
}

bool Slave::Config(ec_domain_t* domain)
{
    if(m_SyncInfo_ == NULL)
    {
        // 1. xml no config
        if (!m_EEprom_->GetSyncInfo(&m_SyncInfo_, m_Index_, m_Master_))
        {
            std::cout << "get sync manage failed" << std::endl;
            return false;
        }
    }

    if(!SetPdoEntry(m_SyncInfo_))
    {
        return false;
    }

#ifdef DEBUG_PDO
    printPdoEntry();
#endif

    if (m_SyncInfo_->n_pdos == 0)
    {
        return true;
    }

    if(ecrt_slave_config_pdos(m_SlvConfig_, EC_END, m_SyncInfo_))
    {
        std::cout << __func__ << ": ecrt_slave_config_pdos fail, index: " << m_Index_ << std::endl;
        return false;
    }

    if(ecrt_domain_reg_pdo_entry_list(domain, m_PdoEntry_))
    {
        std::cout << __func__ << ": ecrt_domain_reg_pdo_entry_list fail, index: " << m_Index_ << std::endl;
        return false;
    }
    return true;
}

bool Slave::SetPdoEntry(ec_sync_info_t *syncInfo)
{
    assert(syncInfo != NULL);
    int pdonum = 0;
    for (int i = 0; syncInfo[i].index != (uint8_t)EC_END; ++ i)
    {
        for (int j = 0; j < syncInfo[i].n_pdos; ++ j)
        {
            pdonum += syncInfo[i].pdos[j].n_entries;
        }
    }

    if(pdonum == 0)
    {
        return true;
    }

    m_PdoEntry_ = new ec_pdo_entry_reg_t[pdonum];
    m_PdoEntryOffset_ = new unsigned int[pdonum];
    m_PdoBitPos_ = new unsigned int[pdonum];

    memset(m_PdoEntryOffset_, 0, pdonum * sizeof(int));
    memset(m_PdoBitPos_, 0, pdonum * sizeof(int));
    memset(m_PdoEntry_, 0, pdonum * sizeof(ec_pdo_entry_reg_t));
    
    pdonum = 0;
    for (int i = 0; syncInfo[i].index != (uint8_t)EC_END; ++i)
    {
        for(int j = 0; j < syncInfo[i].n_pdos; ++ j)
        {
            for(int k = 0; k < syncInfo[i].pdos[j].n_entries; ++ k, ++pdonum)
            {
                m_PdoEntry_[pdonum].alias = 0;
                m_PdoEntry_[pdonum].position = m_SlvInfo_.position;
                m_PdoEntry_[pdonum].vendor_id = m_SlvInfo_.vendor_id;
                m_PdoEntry_[pdonum].product_code = m_SlvInfo_.product_code;

                m_PdoEntry_[pdonum].index = syncInfo[i].pdos[j].entries[k].index;
                m_PdoEntry_[pdonum].subindex = syncInfo[i].pdos[j].entries[k].subindex;
                m_PdoEntry_[pdonum].offset = m_PdoEntryOffset_ + pdonum;
                m_PdoEntry_[pdonum].bit_position = m_PdoBitPos_ + pdonum;
#ifdef DEBUG_PDO
                std::cout << "[DEBUG][SetPdoEntry]: index = " << std::hex << m_PdoEntry_[pdonum].index << ", subindex = " << m_PdoEntry_[pdonum].subindex << std::endl;
#endif
                // 记录pdo的偏移位置
                if(m_PdoOffset_.find(m_PdoEntry_[pdonum].index) != m_PdoOffset_.end())
                {
                    continue;
                }
                m_PdoOffset_[m_PdoEntry_[pdonum].index] = m_PdoEntry_[pdonum].offset;
            }
        }
    }
    m_PdoNum_ = pdonum;
    return true;
}

void Slave::SetDC(uint32_t cycle1, int32_t shift1, uint32_t cycle2, int32_t shift2)
{
    assert(m_SlvConfig_ != NULL);
    ecrt_slave_config_dc(m_SlvConfig_, 0x0330, cycle1, shift1, cycle2, shift2);
}

uint16_t Slave::GetErrorCode() {}
void Slave::SetType(EcType type) { m_SlaveType_ = type; }
void Slave::SetMode(int8_t mode) { rxpdo_.mode = mode; }
void Slave::Reset() { reset_ = true; }

void Slave::SetEnable(bool enable) { enable_ = enable; }
bool Slave::GetEnable() { return enable_; }
bool Slave::StatusTransite() { return true; }

ec_slave_config_t* Slave::GetConfig() { return m_SlvConfig_; }
int8_t Slave::GetState() { return 0x00; }
EcType Slave::GetType() { return m_SlaveType_; }

void Slave::SetPos(int32_t target) {}
void Slave::Retrieve(uint8_t* domainPtr) {}
void Slave::Publisher(uint8_t* domainPtr) {}

void Slave::printPdoEntry()
{
    printf("RxPdo %d\n", m_SyncInfo_[0].n_pdos);
    for (int i = 0; i < m_SyncInfo_[0].n_pdos; ++i)
    {
        printf("Index: 0x%x, entry num: 0x%x", m_SyncInfo_[0].pdos[i].index,  m_SyncInfo_[0].pdos[i].n_entries);
        for(int j = 0; j < m_SyncInfo_[0].pdos[i].n_entries; ++j)
        {
            printf("index = %x, bit = %x, subindex = %x\n", \
                   m_SyncInfo_[0].pdos[i].entries[j].index, \
                   m_SyncInfo_[0].pdos[i].entries[j].bit_length, \
                   m_SyncInfo_[0].pdos[i].entries[j].subindex);
        }
    }

    printf("TxPdo %d\n", m_SyncInfo_[1].n_pdos);
    for (int i = 0; i < m_SyncInfo_[1].n_pdos; ++i)
    {
        printf("Index: 0x%x, entry num: 0x%x", m_SyncInfo_[1].pdos[i].index,  m_SyncInfo_[1].pdos[i].n_entries);
        for(int j = 0; j < m_SyncInfo_[1].pdos[i].n_entries; ++ j)
        {
            printf("index = %x, bit = %x, subindex = %x\n", \
                    m_SyncInfo_[1].pdos[i].entries[j].index, \
                    m_SyncInfo_[1].pdos[i].entries[j].bit_length, \
                    m_SyncInfo_[1].pdos[i].entries[j].subindex);
        }
    }

    printf("index \t subindex \t offset \t alias \t position \t vendor \t product");
    for (int i = 0; i < m_PdoNum_; ++i)
    {
        printf("0x%x \t 0x%x \t 0x%x \t 0x%x \t 0x%x \t 0x%x \t 0x%x \t", \
               m_PdoEntry_[i].index, \
               m_PdoEntry_[i].subindex, \
               *m_PdoEntry_[i].offset, \
               m_PdoEntry_[i].alias, \
               m_PdoEntry_[i].position, \
               m_PdoEntry_[i].vendor_id,
               m_PdoEntry_[i].product_code);
    }
}
